#include "pid.h"
#include "control.h"
#include "motor.h"

#include <math.h>

// float pid_calSpeed(Pid_StructDef* pid_info)
// {
//     float a = 0.8;

//     if (fabs(pid_info->curActual - pid_info->preActual) > fabs(pid_info->preActual * (3.0 / 5)))
//     {
//         return pid_info->preOut;
//     }

//     pid_info->error0 = pid_info->target - pid_info->curActual;
//     pid_info->error0 = (1 - a) * pid_info->error0 + a * pid_info->error1;
//     pid_info->error1 = pid_info->error0;


//     float c = 1 / (0.0008 * fabs(pid_info->error0) + 1);

//     pid_info->errorInt += c * pid_info->error0;

//     if (pid_info->errorInt > PWM_MAX / pid_speedKi) {
//         pid_info->errorInt = PWM_MAX / pid_speedKi;
//     }
//     if (pid_info->errorInt < PWM_MIN / pid_speedKi) {
//         pid_info->errorInt = PWM_MIN / pid_speedKi;
//     }

//     float out = pid_speedKp * pid_info->error0 + pid_speedKi * pid_info->errorInt +
//                 pid_speedKd * (pid_info->error0 - pid_info->error1);

//     if (out > PWM_MAX) {
//         out = PWM_MAX;
//     }
//     if (out < PWM_MIN) {
//         out = PWM_MIN;
//     }
//     pid_info->preOut = out;

//     return out;
// }

float pid_calSpeed(PidSpeedInfo_StructDef* info)
{
    // static float error0   = 0;
    // static float error1   = 0;
    // static float errorInt = 0;

    pid_speedKi = pid_speedKp / 200.0;

    float a = 0.7;

    // if (fabs(info->curActualL - info->preActualL) > fabs(info->preActualL) * (3.0 / 5) ||
    //     fabs(info->curActualR - info->preActualR) > fabs(info->preActualR) * (3.0 / 5))
    // {
    //     return info->preOut;
    // }

    info->error0 = (info->curActualL + info->curActualR) - info->target;
    info->error0 = (1 - a) * info->error0 + a * info->error1;
    info->error1 = info->error0;

    if (fabs(pid_speedKi) > 0.00001) {
        info->errorInt += info->error0;
    }
    else {
        info->errorInt = 0;
    }
    if (info->errorInt > ANGLE_MAX / pid_speedKi) {
        info->errorInt = ANGLE_MAX / pid_speedKi;
    }
    if (info->errorInt < ANGLE_MIN / pid_speedKi) {
        info->errorInt = ANGLE_MIN / pid_speedKi;
    }

    float out = pid_speedKp * info->error0 + pid_speedKi * info->errorInt;
    if (out > ANGLE_MAX) {
        out = ANGLE_MAX;
    }
    if (out < ANGLE_MIN) {
        out = ANGLE_MIN;
    }

    info->preOut = out;
    return out;
}

float pid_calStand(float targetAngle, float roll, float gyroy)
{
    float out = pid_standKp * (targetAngle - roll) - pid_standKd * gyroy;

    if (out > PWM_MAX) {
        out = PWM_MAX;
    }
    if (out < PWM_MIN) {
        out = PWM_MIN;
    }

    return out;
}

float pid_calTurn(float targetTurn, float gyroz)
{
    float out = pid_turnKp * targetTurn + pid_turnKd * gyroz;
    if (out > PWM_MAX) {
        out = PWM_MAX;
    }
    if (out < PWM_MIN) {
        out = PWM_MIN;
    }
    return out;
}
